


// PID class to be written by you
public class PID {

	// Private attribute containing a reference to the GUI
	//@SuppressWarnings("unused")
	//private PIDGUI GUI;
	// Private attribute containing a reference to the PIDParameters
	// currently used
	private PIDParameters pidp;

	// Additional attributes
	private double I; // Integrator state
	private double D; //Derivator state

	private double v; // Desired control signal
	private double e; // Current control error

	private double ad,bd, yOld; // for D
	
	// Constructor
	public PID(String name) {
		PIDParameters p = new PIDParameters();
		p.Beta = 1.0;
		p.H = 0.01;
		p.integratorOn = false;
		p.K = -0.1;
		p.Ti = 0.0;
		p.Tr = 10.0;
		p.Td = 1.7;
		p.N = 7;
		
		//GUI = new PIDGUI(this, p, name);
		setParameters(p);

		this.I = 0.0;
		this.D = 0.0;
		
		this.ad = p.Td / (p.Td + p.N*p.H);
		this.bd = p.K*this.ad*p.N;
		
		this.v = 0.0;
		this.e = 0.0;
	}

	// Calculates the control signal v. Called from BeamAndBallRegul.
	public synchronized double calculateOutput(double yball, double yref){
		this.e = yref - yball;
		this.D = this.ad * this.D - this.bd * (yball - this.yOld);
		this.v = pidp.K * (pidp.Beta * yref - yball) + I + D; // I is 0.0 if integratorOn is false
		this.yOld = yball;
		return this.v;
	}

	// Updates the controller state. Should use tracking-based anti-windup
	// Called from BeamAndBallRegul.
	public synchronized void updateState(double u){
		  if (pidp.integratorOn) {
			  I = I + (pidp.K * pidp.H / pidp.Ti) * e + (pidp.H / pidp.Tr) * (u - this.v);
		  } else {
			  I = 0.0;
		  }
		  
	}

	// Returns the sampling interval expressed as a long. Explicit type casting
	// needed
	public synchronized long getHMillis() {
		return (long) (pidp.H * 1000.0);
	}

	// Sets the PIDParameters. Called from PIDGUI. Must clone newParameters.
	public synchronized void setParameters(PIDParameters newParameters){
		
		pidp = (PIDParameters)newParameters.clone();
		
		this.ad = pidp.Td / (pidp.Td + pidp.N*pidp.H);
		this.bd = pidp.K*this.ad*pidp.N;
		
	}

}